package bme.andraskindler.treenventory.imagemanipulation;

import org.opencv.core.Mat;

import com.google.android.gms.maps.model.LatLng;

/**
 * Térbeli pontot reprezentáló osztály.
 * 
 * @author andraskindler
 * 
 */
public class Vector {

	public double X, Y, Z;

	public Vector() {
	}

	public Vector(double a, double b, double c) {
		super();
		this.X = a;
		this.Y = b;
		this.Z = c;
	}

	/**
	 * Vektorok közötti kivonás művelete ír le.
	 * 
	 * @param other
	 * @return
	 */
	public Vector minus(final Vector other) {
		return new Vector(this.X - other.X, this.Y - other.Y, this.Z - other.Z);
	}

	/**
	 * Kiszámítja azt a pontot, mely két 3D egyenes közötti legrövidebb szakasz középpontján található.
	 * 
	 * @param a1
	 * @param a2
	 * @param b1
	 * @param b2
	 * @return
	 */
	public static Vector closestPoint(final Vector a1, final Vector a2, final Vector b1, final Vector b2) {
		final Vector p13 = a1.minus(b1);
		final Vector p43 = b2.minus(b1);
		final Vector p21 = a2.minus(a1);

		final double d1343 = p13.X * (double) p43.X + (double) p13.Y * p43.Y + (double) p13.Z * p43.Z;
		final double d4321 = p43.X * (double) p21.X + (double) p43.Y * p21.Y + (double) p43.Z * p21.Z;
		final double d1321 = p13.X * (double) p21.X + (double) p13.Y * p21.Y + (double) p13.Z * p21.Z;
		final double d4343 = p43.X * (double) p43.X + (double) p43.Y * p43.Y + (double) p43.Z * p43.Z;
		final double d2121 = p21.X * (double) p21.X + (double) p21.Y * p21.Y + (double) p21.Z * p21.Z;

		final double denominator = d2121 * d4343 - d4321 * d4321;
		final double numerator = d1343 * d4321 - d1321 * d4343;

		final double mua = numerator / denominator;
		final double mub = (d1343 + d4321 * (mua)) / d4343;

		final Vector closestpoint1 = new Vector(a1.X + mua * p21.X, a1.Y + mua * p21.Y, a1.Z + mua * p21.Z);
		final Vector closestpoint2 = new Vector(b1.X + mub * p43.X, b1.Y + mub * p43.Y, b1.Z + mub * p43.Z);

		final double halfdistX = Math.abs((closestpoint1.X - closestpoint2.X) / 2);
		final double halfdistY = Math.abs((closestpoint1.Y - closestpoint2.Y) / 2);
		final double halfdistZ = Math.abs((closestpoint1.Z - closestpoint2.Z) / 2);

		return new Vector(closestpoint1.X + halfdistX, closestpoint1.Y + halfdistY, closestpoint1.Z + halfdistZ);
	}

	/**
	 * A kamera és a fa közötti távolságot számítja ki egy Pithagorasz-tétellel.
	 * 
	 * @param side
	 *            az oldalt jelképező szakasz
	 * @param width
	 *            a fa szélessége
	 * @return
	 */
	public static double calculateCamTreeDistance(final Ray side, final double width) {
		return Math.sqrt(Math.pow(side.length(), 2) - Math.pow(width / 2 - 60, 2));
	}

	/**
	 * A {@link LatLng} típusú koordinátát eltolja a megadott távolsággal és iránnyal.
	 * 
	 * @param distance
	 *            - távolság
	 * @param origpoint
	 *            - kiindulási koordináták
	 * @param angle
	 *            - szög
	 * @return
	 */
	public static LatLng addCamTreeDistance(final double distance, final LatLng origpoint, final double angle) {
		final double distanceNorth = Math.sin(angle) * distance / 1000;
		final double distanceEast = Math.cos(angle) * distance / 1000;

		final double earthRadius = 6371000;

		final double newLat = origpoint.latitude + (distanceNorth / earthRadius) * 180 / Math.PI;
		final double newLon = origpoint.longitude + (distanceEast / (earthRadius * Math.cos(newLat * 180 / Math.PI))) * 180 / Math.PI;

		return new LatLng(newLat, newLon);
	}

	/**
	 * Két pont közötti távolságot számít ki.
	 * 
	 * @param s
	 * @param t
	 * @return
	 */
	public static double calculateDistance(final Vector s, final Vector t) {
		return Math.sqrt(Math.pow(s.X - t.X, 2) + Math.pow(s.Y - t.Y, 2) + Math.pow(s.Z - t.Z, 2));
	}

	/**
	 * Kétdimenziós képbeli pontot 3D ponttá alakít.
	 * 
	 * @param cameramatrix
	 *            - a kameramátrix
	 * @param x
	 *            - a bemeneti pont X koordinátája
	 * @param y
	 *            - a bemeneti pont Y koordinátája
	 * @return
	 */
	public static Vector imagepointToCam(Mat cameramatrix, double x, double y) {

		double vx = x - cameramatrix.get(0, 2)[0];
		double vy = y - cameramatrix.get(1, 2)[0];
		double vz = cameramatrix.get(0, 0)[0];

		return new Vector(vx, vy, vz);
	}

	@Override
	public String toString() {
		return "Vector [X=" + X + ", Y=" + Y + ", Z=" + Z + "]";
	}

}
